/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *  @file palletcalcute.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef PALLETCALCULATE_H
#define PALLETCALCULATE_H

#include "coordinate.h"
#include "domparser.h"
#include "GeneralDefine.h"
#include "pointdata.h"
#include "messagelog.h"

using namespace KDL;

//示教点数目类型，2017-4-18 彭秋明添加
enum STACK_TEACHNUMBER_TYPE
{
    ENUM_STACK_TEACHNUMBER_RCL,  //原点、行点、列点、层点，4个示教点
    ENUM_STACK_TEACHNUMBER_RC,    //原点、行点、列点，3个示教点
    ENUM_STACK_TEACHNUMBER_RL,    //原点、行点、层点，3个示教点
    ENUM_STACK_TEACHNUMBER_CL,   //原点、 列点、层点，3个示教点
    ENUM_STACK_TEACHNUMBER_R,    //原点、行点，2个示教点
    ENUM_STACK_TEACHNUMBER_C,   //原点、 列点，2个示教点
    ENUM_STACK_TEACHNUMBER_L    //原点、 层点，2个示教点
};

//码垛间隔示教类型,2017-3-17 彭秋明添加
enum STACK_INTERVAL_TYPE
{
    ENUM_STACK_INTERVAL_NONE,   //非间隔示教
    ENUM_STACK_INTERVAL_ROW,   //指定码垛行间隔示教
    ENUM_STACK_INTERVAL_COL,   //指定码垛列间隔示教
    ENUM_STACK_INTERVAL_LAYER,   //指定码垛层间隔示教
    ENUM_STACK_INTERVAL_RC,   //指定码垛行、列间隔示教
    ENUM_STACK_INTERVAL_RL,   //指定码垛行、层间隔示教
    ENUM_STACK_INTERVAL_CL,   //指定码垛列、层间隔示教
    ENUM_STACK_INTERVAL_RCL  //指定码垛行列层间隔示教
};
// 2017-3-31 彭秋明添加，码垛/拆垛
enum STACK_UPDOWN_TYPE
{
    ENUM_STACK_UP,  //码垛
    ENUM_STACK_DOWN  //拆垛
};
// 2017-3-31 彭秋明添加，码垛/拆垛顺序
enum STACK_ORDER_STATE
{
    ENUM_STACK_ORDER_RCL,  //行列层顺序
    ENUM_STACK_ORDER_CRL  //列行层顺序
};
// 2017-3-31 彭秋明添加，码垛/拆垛是否完成的状态
enum STACK_WORK_STATE
{
    ENUM_STACK_WORK_DOING,  //码垛
    ENUM_STACK_WORK_FINISH   //拆垛
};

struct PalletSave
{
    Vector xAxisVector;
    Vector yAxisVector;
    Vector zAxisVector;
    Frame btmFrame;//堆叠点
    QVector<Frame> pointAnFrame;//接近点
    QVector<Frame> pointRnFrame;//远离点
    int row;//行，从0开始
    int col;//列
    int layer;//层
    std::string configString;//多解多圈，同一个码垛的示教点多解多圈一样
    //2017-3-20 彭秋明添加
    PointPro btmPointPro;//堆叠点
    QVector<PointPro> pointAnPointPro;//接近点
    QVector<PointPro> pointRnPointPro;//远离点
    int finishOutput;
    // 2017-3-25 彭秋明添加
    int firstNum; //初始化时码垛当前点标识位
    int modifyPlValue; //PL寄存器修改时的标识位

};

//2017-4-22 彭秋明添加，存放码垛的行列层
struct RowColLayer
{
    int row;
    int col;
    int layer;
};

class PalletCalculate:  public DomParser
{
private:
    PointData* pointData;//
    Coordinate* coordinate;//
    QVector<PalletSave> palletVector;//存码垛计算数据的向量
    // 2017-3-21 彭秋明添加,存放码垛计算数据的中间变量
    PalletSave currentTmpPallet;
    //2017-3-17 彭秋明添加
    int teachIntervalType; //间隔示教类型
    int number;
    int teachNumberType; //示教点数量类型

    // 2017-3-27 彭秋明添加
    QVector <StackInfo> palletList;
    QVector<RowColLayer> palletRCL; //存放码垛初始点的行列层
    QVector<RowColLayer> palletPlRCL; //存放码垛PL的行列层
    pthread_mutex_t palletMutex; //码垛线程锁
    pthread_mutexattr_t palletAttr;
    int row;
    int col;
    int layer;

public:
    int palletAmount;//码垛数量

public:
//    PalletCalculate(PointData* _pointData, Coordinate* _coordinate);
    //2017-3-31 彭秋明修改
    PalletCalculate();
    ~PalletCalculate();

    int palletInput(PointData* _pointData, Coordinate* _coordinate);
    /**
     * @brief frameToPointPro,将Frame转换成PointPro
     * @param palletIndex，码垛号
     * @param anAmount，接近点数量
     * @param rnAmount，逃离点数量
     * @param tmpPallet
     */
    void frameToPointPro(int palletIndex, int anAmount, int rnAmount, PalletSave &tmpPallet);
    /**
     * @brief cartToPointProJoint,将PointPro类型的笛卡尔转为关节
     * @param pointPro，输入参数
     * @return ，返回转换后的PointPro类型
     */
    PointPro cartToPointProJoint(PointPro pointPro);
    /**
     * @brief initStackInfo 按palletIndex初始化码垛
     * @param palletIndex 码垛号
     * @return
     */
    int initStackInfo(int palletIndex);
    /**
     * @brief initStackInfo_B
     * @param stackInfo
     * @return
     */
    int initStackInfo_B(struct StackInfo stackInfo, int palletIndex);
    /**
     * @brief calculatePallet 计算码垛接近点/堆叠点/逃离点
     * @param palletIndex 码垛号,从0开始
     * @param row 行
     * @param col 列
     * @param layer 层
     * @return 1:collect; -1:palletIndex exceed; -2: row/col/layer exceed
     */
    int calculatePallet(int palletIndex, int row, int col, int layer);
    int calculateNext(int palletIndex);
    int getPointData(PalletPoint palletPoint, PointPro &f);
    // 2017-4-20 彭秋明添加
    /**
     * @brief setPalletFirstNum 将对应码垛号的firstNum参数置0
     * @param palletIndex 码垛号
     * @return
     */
    int setPalletFirstNum(int palletIndex);//2017-6-19.xyf.vrep 出现断错误

    /**
     * @brief setPalletPlValue 将对应码垛号的modifyPlValue参数置1
     * @param palletIndex 码垛号
     * @return
     */
    int setPalletPlValue(int palletIndex);

    /**
     * @brief readPalletPlValue,读取ＰL的行列层信息
     * @param palletIndex, 码垛号
     * @param plRow，ＰL的行
     * @param plCol，ＰL的列
     * @param plLayer，ＰL的层
     * @return
     */
    int readPalletPlValue(int palletIndex, int plRow, int plCol, int plLayer);

    /**
     * @brief returnPalletVector,获取指定码垛号的码垛计算信息
     * @param palletIndex，码垛号
     * @return
     */
    PalletSave returnPalletVector(int palletIndex);

private:
    void getNextPalletInfo(int palletIndex);
    void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int _robotId,
                int parameter1 = 0,
                string parameter2 = "",
                int parameter3 = 0,
                int parameter4 = 0);
};

#endif // PALLETCALCULATE_H
